//
// Created by wsl on 23-4-6.
//
#include "circle_detect.h"

#include <utility>

// 计算直线与圆的交点
cv::Point getIntersection(Circle circle, cv::Point p) {
    double x0 = circle.center.x;
    double y0 = circle.center.y;
    double x1 = p.x;
    double y1 = p.y;
    double r = circle.radius;
    // 计算圆心与点的向量
    double vx = x1 - x0;
    double vy = y1 - y0;

    // 计算向量的长度
    double d = sqrt(vx * vx + vy * vy);

    if (d < r) {
        // 点在圆内，返回该点
        return p;
    } else {
        // 计算交点的坐标
        double ux = vx / d;
        double uy = vy / d;
        double px = x0 + r * ux;
        double py = y0 + r * uy;
        return cv::Point{int(px), int(py)};
    }
}

std::vector<cv::Point> getPointsOnLine(const Circle& c0, cv::Point p) {
    std::vector<cv::Point> points{};
    auto p0= getIntersection(c0,std::move(p));
    Line line0;
    line0.cal_param(c0.center,p0);
    if(p0.x>c0.center.x)
    {
        for(int x=p0.x;x>c0.center.x;x--)
        {
            double y=line0.k*x+line0.b;
            points.emplace_back(x,int(y));
        }
    } else
    {
        for(int x=p0.x;x<c0.center.x;x++)
        {
            double y=line0.k*x+line0.b;
            points.emplace_back(x,int(y));
        }
    }

    return points;
}

cv::Mat tif2png(cv::Mat &gray) {
    gray.convertTo(gray,CV_64F);
    cv::Mat res=gray.clone();
    double v_max=-1;
    double v_min=99999;
    for (int row = 0; row < gray.rows; row++) {
        double *curr_depth = gray.ptr<double>(row);
        for (int col = 0; col < gray.cols; col++) {
            double z = curr_depth[col];
            if(z>0)
            {
                if(z>v_max)
                {
                    v_max=z;
                }
                if(z<v_min)
                {
                    v_min=z;
                }
            }
        }}

    for (int row = 0; row < res.rows; row++) {
        auto *curr_depth = res.ptr<double>(row);
        for (int col = 0; col < res.cols; col++) {
            auto z = curr_depth[col];
            if(z>0)
            {
                auto cc=(z-v_min)/(v_max-v_min);
                curr_depth[col]=cc*255;
            }
        }}
    res.convertTo(res,CV_8UC1);
    return res;

}
bool comparePoints(const cv::Point& p1, const cv::Point& p2)
{
    return p1.y < p2.y;
}
std::vector<cv::Point> get_circle(cv::Mat mask) {
    std::vector<cv::Mat> imgs{};
    std::vector<cv::Point> centers;
    cv::split(mask,imgs);
    mask=imgs[0];
    // 定义轮廓和层次结构
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;

    // 查找轮廓
    findContours(mask, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
    cv::cvtColor(mask,mask,cv::COLOR_GRAY2BGR);
    // 计算每个轮廓的重心
    for (int i = 0; i < contours.size(); i++)
    {
        if(cv::contourArea(contours[i])<50000)
        {
            continue;
        }
        cv::Moments mu = moments(contours[i], false);
        cv::Point2f centroid(mu.m10 / mu.m00, mu.m01 / mu.m00);
        centers.emplace_back(int(centroid.x),int(centroid.y));
    }
    std::sort(centers.begin(), centers.end(), comparePoints);
    return centers;

}

void AffShift(cv::Mat& aff, cv::Point2f& sh) {
    aff.at<double>(0, 2) += sh.x;		aff.at<double>(1, 2) += sh.y;
}


cv::Mat getMatrix(cv::Point2f c1, cv::Point2f c2, double s)
{
    cv::Mat aff = getRotationMatrix2D(c1, 0, 1);
    aff *= s;
    cv::Point2f c3=c1-c2*s;
    AffShift(aff, c3);
    return aff;

}

cv::Mat calMatrix(cfg3D cfg) {
    assert(cfg.src_camera==4||cfg.src_camera==5);
    assert(cfg.dst_camera==0||cfg.dst_camera==1);
    if(cfg.dst_camera==1)//flip for camera1
    {
        cfg.dst_circle.x=cfg.dst_size.width-cfg.dst_circle.x;
    }
    // flip and rot 90 for 2D image
    cfg.src_circle.x=cfg.src_size.width-cfg.src_circle.x;
    double x=cfg.src_circle.x;
    cfg.src_circle.x=cfg.src_circle.y;
    cfg.src_circle.y=cfg.src_size.width-x;

    double scale=cfg.rS[cfg.dst_camera]*1.0/cfg.rS[cfg.src_camera-2];
    cv::Mat m= getMatrix(cfg.dst_circle,cfg.src_circle,scale);

    cv::Mat flip_m=cv::Mat::eye(cv::Size(3,3),CV_64F);
    cv::Mat rot_m=cv::Mat::zeros(cv::Size(3,3),CV_64F);

    flip_m.at<double>(0,0)=-1;
    flip_m.at<double>(0,2)=cfg.src_size.width;

    rot_m.at<double>(0,1)=1;
    rot_m.at<double>(1,0)=-1;
    rot_m.at<double>(2,2)=1;
    rot_m.at<double>(1,2)=cfg.src_size.width;
    m=m*(rot_m*flip_m);

    cv::Mat m33=cv::Mat::eye(cv::Size(3,3),CV_64F);
    auto roi=cv::Rect(0,0,3,2);
    copyTo(m,m33(roi),cv::Mat::ones(cv::Size(3,2),CV_8U));
    m=m33;
    if(cfg.dst_camera==1)
    {
        flip_m.at<double>(0,2)=cfg.dst_size.width;
        m=flip_m*m33;
    }

    return m;
}

cv::Mat calMatrix(cv::Mat &dst_mask, cv::Mat &src_mask, int src_camera, int dst_camera) {
    auto dst_circles= get_circle(dst_mask);
    auto src_circles= get_circle(src_mask);
    assert(dst_circles.size()==2);
    assert(src_circles.size()==1);

    cfg3D cfg;
    cfg.src_size=src_mask.size();
    cfg.dst_size=dst_mask.size();
    cfg.src_camera=src_camera;
    cfg.dst_camera=dst_camera;
    cfg.src_circle=src_circles[0];
    if(src_camera==4)
    {
        cfg.dst_circle=dst_circles[1];
    } else
    {
        cfg.dst_circle=dst_circles[0];
    }
    auto m= calMatrix(cfg);
    return m;
}



int Line::cal_param(const cv::Point& a, const cv::Point& bb) {
    this->k=(a.y-bb.y)/(a.x-bb.x+1e-10);
    this->b=bb.y- this->k*bb.x;
    return 0;
}

std::vector<cv::Point> getPolysOnCircle(const Circle& cmax,const Circle& cmin,cv::Point p1,cv::Point p2)
{
    auto pt1= getIntersection(cmax,p1);
    auto pt2= getIntersection(cmin,p1);
    auto pt3= getIntersection(cmax,p2);
    auto pt4= getIntersection(cmin,p2);
    return {pt1,pt2,pt4,pt3};
}
